package comunicacion.simulador;

import java.io.IOException;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;

import javax.vecmath.Point3d;

import comunicacion.Conector;

public class Simulador implements Conector {
	private static final double BETA = 2.8;
	private static final double GAMMA = 1.0;
	private static final double H = 2.0;
	
	private static final double MAXIMA_VELOCIDAD_DIRECT = 1.0;
	private static final double TOLERANCIA_ANGULO = 0.005;
	private static final double DISTANCIA_ACEPTABLE = 0.08;
	
	private int idRobot;
	private int motorIzquierdo;
	private int motorDerecho;
	
	private DatagramSocket simSocket;
	private InetAddress serverIP;
	private int serverPort;
	
	private Point3d ultimasCoordenadas;
	
	public Simulador(String ipServidor, int puertoServidor, int idRobot, int idMotorIzquierdo, int idMotorDerecho) throws IOException {
		this.serverIP = InetAddress.getByName(ipServidor);
		this.serverPort = puertoServidor;
		
		this.idRobot = idRobot;
		this.motorIzquierdo = idMotorIzquierdo;
		this.motorDerecho = idMotorDerecho;
		
		this.simSocket = new DatagramSocket();

		// Enviamos la solicitud para la creacion del robot y el campo de juego
		String mensajeCreacion = LectorArchivo.leer("CreacionAgenteEntorno.xml");
		int puertoLocal = simSocket.getLocalPort();
		mensajeCreacion = mensajeCreacion.replaceAll("#puertoLocal#", String.valueOf(puertoLocal));
		mensajeCreacion = mensajeCreacion.replaceAll("#idRobot#", String.valueOf(idRobot));
		mensajeCreacion = mensajeCreacion.replaceAll("#motorIzquierdo#", String.valueOf(motorIzquierdo));
		mensajeCreacion = mensajeCreacion.replaceAll("#motorDerecho#", String.valueOf(motorDerecho));
		DatagramPacket paquete = new DatagramPacket(mensajeCreacion.getBytes(), mensajeCreacion.length(), serverIP, serverPort);
		simSocket.send(paquete);
		
		
		Point3d posicion = new Point3d(1.5, 1.5, -1.578);
		
		///ESTO ES SOLAMENTE ACA
		String mensajePosicion = LectorArchivo.leer("ReubicarEntidad.xml");
		mensajePosicion = mensajePosicion.replaceAll("#idRobot#", String.valueOf(idRobot));
		mensajePosicion = mensajePosicion.replaceAll("#pos_x#", String.valueOf(posicion.x));
		mensajePosicion = mensajePosicion.replaceAll("#pos_y#", String.valueOf(posicion.y));
		mensajePosicion = mensajePosicion.replaceAll("#rotacion#", String.valueOf(posicion.z));
		paquete = new DatagramPacket(mensajePosicion.getBytes(), mensajePosicion.length(), serverIP, serverPort);
		simSocket.send(paquete);
		
//		// Indicamos el inicio de la simulacion
//		String mensajeInicio = LectorArchivo.leer("IniciarSimulacion.xml");
//		paquete = new DatagramPacket(mensajeInicio.getBytes(), mensajeInicio.length(), serverIP, serverPort);
//		simSocket.send(paquete);
		
		// TODO esto no se si es prolijo, ver que devolver en caso de que no se recibe nada desde el sim
		ultimasCoordenadas = new Point3d(1.5, 1.5, -1.578);		
	}
	
	@Override
	public void detenerMotores() throws IOException {
		this.enviarVelocidades(0.0, 0.0);		
	}
	
	@Override
	public void enviarVelocidades(double velocidadIzquierda, double velocidadDerecha) throws IOException {
		String velocidades = LectorArchivo.leer("Velocidades.xml");
		
		velocidades = velocidades.replaceAll("#idRobot#", String.valueOf(idRobot));
		velocidades = velocidades.replaceAll("#motorIzquierdo#", String.valueOf(motorIzquierdo));
		velocidades = velocidades.replaceAll("#velocidadIzquierda#", String.valueOf(velocidadIzquierda));
		velocidades = velocidades.replaceAll("#motorDerecho#", String.valueOf(motorDerecho));
		velocidades = velocidades.replaceAll("#velocidadDerecha#", String.valueOf(velocidadDerecha));
		
		DatagramPacket paquete = new DatagramPacket(velocidades.getBytes(), velocidades.length(), serverIP, serverPort);
		simSocket.send(paquete);
	}	

	@Override
	public Point3d getCoordenadas(int idEntidad) throws IOException {
		DatagramPacket paquete = new DatagramPacket(new byte[40000], 40000);
		simSocket.receive(paquete);
		
		// Obtenemos el mensaje recibido cargado en un string
		String mensaje = new String(paquete.getData(), 0, paquete.getData().length-1).trim();
		
		Point3d coordenadas = null;
		
		// Vemos si es un broadcast, o si no es la informacion que estamos esperando
		if (mensaje.substring(1, new String("WorldBroadcast").length()+1).equals("WorldBroadcast")) {
			// Desde el mensaje que recibimos desde el simulador, parseamos los datos del robot
			Robot robot = ParserXml.getRobot(mensaje, idRobot);
			// Interesan las coordenas de la posicion (x,y); asi como tambien la rotacion
			coordenadas = new Point3d(robot.getPosX(), robot.getPosY(), robot.getRotZ());
			
			ultimasCoordenadas = new Point3d(coordenadas.x, coordenadas.y, coordenadas.z);
		} else {
			// Si no tenemos nuevos datos, devolvemos la ultima lectura
			coordenadas = new Point3d(ultimasCoordenadas.x, ultimasCoordenadas.y, ultimasCoordenadas.z);
		}
		
		return coordenadas;
	}
	
	@Override
	public Point3d[] getCoordenadasObjetos() throws IOException {
		
		DatagramPacket paquete = new DatagramPacket(new byte[40000], 40000);
		simSocket.receive(paquete);
		
		// Obtenemos el mensaje recibido cargado en un string
		String mensaje = new String(paquete.getData(), 0, paquete.getData().length-1).trim();
		
		Point3d[] coordenadas = null;
		
		// Vemos si es un broadcast, o si no es la informacion que estamos esperando
		if (mensaje.substring(1, new String("WorldBroadcast").length()+1).equals("WorldBroadcast")) {
			System.out.println(mensaje);
			System.out.println();
			
			// Desde el mensaje que recibimos desde el simulador, parseamos los datos del robot
			Robot[] robots = ParserXml.getRobots(mensaje, idRobot);
			if (robots.length > 1) {
				//Hay alguno ademas de mi robot
				coordenadas = new Point3d[robots.length-1];
				int tope = 0; 
				for (int i=0; i<robots.length; i++) {
					if (robots[i].getEntityId() != idRobot) {
						// Interesan las coordenas de la posicion (x,y); asi como tambien la rotacion
						Point3d unaCoord = new Point3d(robots[i].getPosX(), robots[i].getPosY(), robots[i].getRotZ());
						coordenadas[tope++] = unaCoord;
					}
				}	
			} 			
		} else {
			System.out.println("*** Llego algo que no es un broadcast");
		}
		
		return coordenadas;
		
		
//		int cantobstaculos = 1;
//		Point3d[] res = new Point3d[cantobstaculos];
//		//res[0] = new Point3d(1.1, 1.2, Math.PI/2.0);
//		//res[0] = new Point3d(1.1, 0.9, Math.PI/2.0);
//		//res[0] = new Point3d(1.2, 1.1, Math.PI/2.0);
//	//	res[0] = new Point3d(1.0, 0.4, Math.PI/2.0);
//		//res[1] = new Point3d(1.1, 0.4, -Math.PI/2.0);
//		//res[1] = new Point3d(1.1, 0.1, -Math.PI/2.0);
//		//res[1] = new Point3d(0.8, 0.7, -Math.PI/2.0);
//		res[0] = new Point3d(1.0, 0.6, Math.PI/2.0);
//		//res[2] = new Point3d(1.55, 0.8, 0);
//		//res[2] = new Point3d(1.5, 0.9, Math.PI/2.0);
//		//res[2] = new Point3d(1.2, 0.5, Math.PI/2.0);
//	//	res[2] = new Point3d(1.0, 0.8, Math.PI/2.0);
//		/*for (int i=0; i<cantobstaculos; i++){
//			try {
//				Point3d obstaculo = conector.getCoordenadas(i+2);
//				res[i] = new Point3d(obstaculo.x, obstaculo.y, obstaculo.z);
//			} catch (IOException e) {
//				// TODO Auto-generated catch block
//				e.printStackTrace();
//			}
//		}*/
//		return res;
//		

	}
	
	

	@Override
	public double getDistanciaAceptable() {
		return DISTANCIA_ACEPTABLE;
	}

	
	private void registrarObstaculo(int idEntidad, double masa, double posX, double posY, double rot, double tamX, double tamY, double tamZ) throws IOException {
		String obstaculo = LectorArchivo.leer("CreacionObstaculo.xml");
		
		obstaculo = obstaculo.replaceAll("#idEntidad#", String.valueOf(idEntidad));
		obstaculo = obstaculo.replaceAll("#masa#", String.valueOf(masa));
		double posZ = 0.06 + tamZ/2.0;
		obstaculo = obstaculo.replaceAll("#posX#", String.valueOf(posX));
		obstaculo = obstaculo.replaceAll("#posY#", String.valueOf(posY));
		obstaculo = obstaculo.replaceAll("#posZ#", String.valueOf(posZ));
		
		obstaculo = obstaculo.replaceAll("#rotacion#", String.valueOf(rot));
		
		obstaculo = obstaculo.replaceAll("#sizeX#", String.valueOf(tamX));
		obstaculo = obstaculo.replaceAll("#sizeY#", String.valueOf(tamY));
		obstaculo = obstaculo.replaceAll("#sizeZ#", String.valueOf(tamZ));
		
		System.out.println("Creando:::::"+obstaculo);
		
		DatagramPacket paquete = new DatagramPacket(obstaculo.getBytes(), obstaculo.length(), serverIP, serverPort);
		simSocket.send(paquete);
	}

	@Override
	public double getFactorBeta() {
		return BETA;
	}

	@Override
	public double getFactorGamma() {
		return GAMMA;
	}

	@Override
	public double getFactorH() {
		return H;
	}

	@Override
	public double getMaximaVelocidadDirect() {
		return MAXIMA_VELOCIDAD_DIRECT;
	}

	public double getToleranciaAngulo() {
		return TOLERANCIA_ANGULO;
	}

}
